/*******************************************************************************
Archivo: Control del Motor.c
Descripción: Programa que controla el motor que mueve el ventilador en el tunel
de viento de la UVG. Parte del trabajo de graduacion "Sistema electromecánico 
para el control de la velocidad del viento en el túnel de viento de la 
Universidad del Valle de Guatemala".
Autor: Josue Samuel Barrientos Castañeda
Compilador: Mikroc
Microcontrolador: PIC18F45K22
Fecha de modificacion: 14.10.2013
*******************************************************************************/

//********************Definición de Variables***********************************
const Dir=0x50, Mina=28, Mind=51;
unsigned short portbtemp, OK=1, cnti2c=0, cntuart=0, next, percentage;
unsigned short comando[2]={0,0}, P1, P2, uart20, uart21, uart22;
unsigned short tempshort, tempshort2, tempi2c, porcentaje_pasado=0;
char txtlong[15], txt[7];
float  corriente, freq, k, Cn, error, vel_pasada, vel_actual, integral;
unsigned int cuartos, tempint, CCP, vel_deseada=14;
signed int presion_actual, derivativo;
signed short tempsshort;
bit automatico, procesar_comandos, sampled, encendido;

//********************Definición de Macros**************************************
#define PARAR() PORTA.RA6=PORTA.RA7=0;
#define ADELANTE() PORTA.RA6=0; PORTA.RA7=1;
#define DETRAS() PORTA.RA6=1; PORTA.RA7=0;
#define PWM(valor) CCPR1L=valor;
#define CONVER(min, per) (255-min)*per/100+min;
#define MICRO_ADELANTE PORTB.RB4!=0
#define MICRO_ATRAS PORTB.RB5!=0
#define ENCENDER_TUNEL() PORTE.RE0=1; delay_ms(1000); PORTE.RE0=0;
#define APAGAR_TUNEL() PORTE.RE1=1; delay_ms(1000);  PORTE.RE1=0;

//********************Inicializacion de Módulos y Puertos***********************
void Inicializar(){                   //Oscilador interno a 8Mhz
  OSCCON.IRCF2=1;
  OSCCON.IRCF1=1;
  OSCCON.IRCF0=0;
  ANSELA  = 0B00000100;               //Configuracion de pines analogicos
  ANSELB = 0;
  ANSELC = 0;
  ANSELD = 0;
  ANSELE = 0;
  C1ON_bit = 0;                       //Comparadores deshabilitados
  C2ON_bit = 0;
  
  PORTE.RE0=0;                        //Configuracion de puertos
  Delay_ms(50);
  PORTE.RE1=0;
  PORTA = 0;
  TRISA = 0b00000100;
  TRISB = 0b11111111;
  TRISC = 0b00011010;
  TRISE = 0b00000100;

  UART1_Init(9600);                   //Inicializar Comunicacion serial a 9600
  Delay_ms(100);

  UART2_Init(9600);                   //Inicializar Comunicacion serial a 9600
  Delay_ms(100);

  PWM1_Init(500);                     //Inicializar PWM a 50Hz
  Delay_ms(100);
  PWM(0)
  PWM1_Start();

  SSP1ADD =  Dir;                     //Direccion de esclavo I2C
  SSP1CON1 = 0x36;                    //Configuracion I2C esclavo 7bit adress
  Delay_ms(100);

  ADC_INIT();
  Delay_ms(100);

  CCP2CON=0b00000101;                 //Configuracion de modulo CCP2 en modo
                                            //capture para cada flanco de subida
  T1CON.TMR1CS1 = 0;                  //Configuracion del TMR1.
  T1CON.TMR1CS0 = 0;                  //Prescaler= 8 | Precarga = 0
  T1CON.T1CKPS1 = 1;                  //Frecuencia de interrupcion = 3.81 Hz
  T1CON.T1CKPS0 = 1;                  //Periodo de interrupcion = 0.262144 s
  T1CON.T1OSCEN = 0;
  T1CON.T1SYNC = 0;
  T1CON.T1RD16=0;
  T1CON.TMR1ON = 1;

  T0CON.TMR0ON=1;
  T0CON.T08BIT=0;
  T0CON.T0CS = 0;                     //Configuracion del TMR1.
  T0CON.T0SE = 0;                     //Usado para rebote de los microswitches
  T0CON.PSA = 1;                      //Prescaler= 256 | Precarga = 49675
  T0CON.T0PS2 = 0;                    //Frecuencia de interrupcion = 33.10 Hz
  T0CON.T0PS1 = 0;                    //Periodo de interrupcion = 0.030208 s
  T0CON.T0PS0 = 0;
  TMR0H=0xC2;
  TMR0L=0x0B;

  PIE3.RC2IE=1;                  //Habilitar interrupcion de recepcion de USART2
  PIE1.RC1IE=1;                  //Habilitar interrupcion de recepcion de USART
  IOCB.RB4=1;                    //Habilitar 'Interrupt on Change' en pin RB4
  IOCB.RB5=1;                    //Habilitar 'Interrupt on Change' en pin RB5
  PIE1.SSPIE = 1;                //Habilitar interrupciones de I2C
  PIE2.CCP2IE=1;                 //Habilitar interrupciones de CCP2
  INTCON.RBIE=1;                 //Habilitar interrupciones del PORTB
  INTCON.PEIE = 1;               //Habilitar interrupciones de perifericos
  INTCON.GIE = 1;                //Habilitar interrupciones
  
  automatico=0;
  procesar_comandos=0;
}

/*******************************************************************************
Función:     Moverse_Continuo
Descripcion: Genera las salidas necesarias para que se mueva el motor en la
             direccion y velocidad deseada. No acepta cambios bruscos de
             velocidad. Solo hace cambios de 5%.
Parametros: Direccion de movimiento 'a'-adelante 'd'-atras
            Porcentaje de velocidad 0-100
Resultado:  El PWM se coloca al porcentaje necesario para dar el porcentaje de
            velocidad y los pines de direccion se configuran.
*******************************************************************************/
void Moverse_Continuo(short dir, unsigned short porcentaje){
  if (porcentaje>100){porcentaje=100;}
  
  tempsshort=porcentaje-porcentaje_pasado;
  if (tempsshort>5){porcentaje=porcentaje_pasado+5;}
  else if (tempsshort<-5){porcentaje=porcentaje_pasado-5;}

  if(dir=='a' && MICRO_ADELANTE){
    tempshort=CONVER(Mina, porcentaje)
    if (PORTA.RA6 && PORTA.RA7==0 && CCPR1L!=0){tempshort=0;porcentaje=1;};
    ADELANTE()
    PWM(tempshort)
  }
  else if (dir=='d' && MICRO_ATRAS){
    tempshort=CONVER(Mind, porcentaje)
    if (PORTA.RA7 && PORTA.RA6==0 && CCPR1L!=0){tempshort=0; porcentaje=1;};
    DETRAS()
    PWM(tempshort)
  }
  UART2_WRITE_TEXT(",, ");
  bytetostr(tempshort, txt);
  UART2_WRITE_TEXT(txt);
  porcentaje_pasado=porcentaje;
}

/*******************************************************************************
Función:     Moverse_Pasos
Descripcion: Mueve el motor por pasos. Un paso es hasta que el encoder genera un
             pulso.
Parametros:  Parametros: Direccion de movimiento 'a'-adelante 'd'-atras
             Porcentaje de velocidad 0-100
             Numero de pasos
Resultado:   La variable cuartos se hace cero. El ventilador se encuentra en la
             posición cero.
*******************************************************************************/
void Moverse_Pasos(char dir, short velocidad, short pasos ){
  tempint=cuartos;

  if(dir=='a' && MICRO_ADELANTE){
    tempint+=-pasos;
    ADELANTE()
    PWM(velocidad)
    while(cuartos!=tempint && MICRO_ADELANTE){}
  }
  else if (dir=='d' && PORTB.RB5!=0){
    tempint+=pasos;
    DETRAS()
    PWM(velocidad)
    while(cuartos!=tempint && MICRO_ATRAS){}
  }

  PARAR()
}


/*******************************************************************************
Función:     Auto_Zero
Descripcion: Mueve el ventilador cerrandolo hasta que se activa el microswitch
             del frente. Primero lo hace rápido, se separa de nuevo y lo hace de
             nuevo lentamente.
Resultado:   La variable cuartos se hace cero. El ventilador se encuentra en la
             posición cero.
*******************************************************************************/
void Auto_Zero(){

  Moverse_Continuo('a', 30);
  while (MICRO_ADELANTE){}
  PARAR()
  delay_ms(250);

  Moverse_Continuo('d', 30);
  delay_ms(500);
  PARAR()

  Moverse_Continuo('a', 10);
  while (MICRO_ADELANTE){}
  PARAR()

  cuartos=0;
}

/*******************************************************************************
Función:      Lobby
Descripcion:  Interpreta los datos recibidos por el puerto serial o por I2C.
Precondicion: La variable comando ha sido cargada con el dato que llegó por el
              puerto
Resultado:    Se settean distintas banderas o se llama a distintas funciones
              segun el valor de la variable comando.
*******************************************************************************/
void Lobby(){
  automatico=0;
  switch (comando[0]){
    case 'w':
      if (CCPR1L!=255){CCPR1L++;}
      break;
    case 's':
      if (CCPR1L!=0){CCPR1L--;}
      break;
    case 'a':
      do {
      Moverse_Continuo('a', comando[1]);
      delay_ms(100);
      }while (comando[1]!=porcentaje_pasado);
      break;
    case 'd':
      do {
      Moverse_Continuo('d', comando[1]);
      delay_ms(100);
      }while (comando[1]!=porcentaje_pasado);
      break;
    case 'e':
      ENCENDER_TUNEL()
      encendido=1;
      break;
    case 'o':
      APAGAR_TUNEL()
      encendido=0;
      break;
    case 'f':
      PARAR();
      porcentaje_pasado=0;
      break;
    case '4':
      PWM(30)
      break;
    case '5':
      PWM(50)
      break;
    case '6':
      PWM(75)
      break;
    case '7':
      PWM(200)
      break;
    case '8':
      PWM(250)
      break;
    case '0':
      PWM(0)
      porcentaje_pasado=0;
      break;
    case 'z':
      Auto_Zero();
      porcentaje_pasado=0;
      break;
    case '+':
      Moverse_Pasos('d', 60, 1);
      porcentaje_pasado=0;
      break;    
      case '-':
      Moverse_Pasos('a', 45, 1);
      porcentaje_pasado=0;
      break;
    case 'm':
      Moverse_Pasos('d', 55, 4);
      porcentaje_pasado=0;
      break;
    case 'n':
      Moverse_Pasos('a', 45, 4);
      porcentaje_pasado=0;
      break;
    case 'p':
      automatico=1;
      PARAR();
      porcentaje_pasado=0;
      vel_deseada=comando[1];
      break;
    case 't':
      vel_deseada+=1;
      automatico=1;
      porcentaje_pasado=0;
      break;
    case 'y':
      vel_deseada-=1;
      automatico=1;
      porcentaje_pasado=0;
      break;
    case 'u':
      vel_deseada=16;
      automatico=1;
      porcentaje_pasado=0;
      break;
    case 'i':
      vel_deseada=12;
      automatico=1;
      porcentaje_pasado=0;
      break;
    case 'o':
      vel_deseada=20;
      automatico=1;
      porcentaje_pasado=0;
      break;
  }
  comando[0]=0;
  comando[1]=0;
  procesar_comandos=0;
}

/*******************************************************************************
Función:      Revisar_Microswitches
Descripcion:  Revisa que los pines a los que se conectan las terminales NC y la
              NO de los microswitches tengan valores distintos. Si son iguales
              indica que hay algun problema.
Resultado:    Se modifica la variable OK lo que hara que el PIC salga del loop
              principal.
*******************************************************************************/
void Revisar_Microswitches(){
  if (PORTB.RB4!=~PORTB.RB1){OK=10;}
  if (PORTB.RB5!=~PORTB.RB2){OK=20;}
}

/*******************************************************************************
Función:      Control_PID
Descripcion:  Implementa un controlador PID para establecer la velocidad de
              viento deseada.
*******************************************************************************/
void Control_PID(){
  vel_actual=1.026*sqrt(presion_actual);
  error=vel_deseada-vel_actual;
  
  derivativo=vel_actual-vel_pasada+0.5;
//  if (MICRO_ADELANTE && MICRO_ATRAS){integral=integral+error;}
//  if (integral>3){integral=0;}
//  else if (error<-3){integral=0;}
  
  if (vel_deseada>=15 && vel_actual>=15){Cn=error*3+derivativo*6;}//+integral;}
  else {Cn=error*12+derivativo*3;}//+integral;}
  
  percentage=abs(Cn)+0.5;
//  if (percentage>100){percentage=100;}

  if (error>0.5 || error<-0.5){
    if (Cn>0){Moverse_continuo('a', percentage);}
    else if(Cn<0){Moverse_continuo('d', percentage);}
  }
  else {PARAR()}

  UART2_WRITE_TEXT("\n\r");
  floattostr(vel_deseada, txtlong);
  UART2_WRITE_TEXT(txtlong);
  UART2_WRITE_TEXT(",");
  //Inttostr(integral, txtlong);
  //UART2_WRITE_TEXT(txtlong);
  //UART2_WRITE_TEXT(",");
  Floattostr(vel_actual, txtlong);
  UART2_WRITE_TEXT(txtlong);
  UART2_WRITE_TEXT(",");
  Bytetostr(percentage, txt);
  UART2_WRITE_TEXT(txt);
  
  vel_pasada=vel_actual;
  sampled=0;
}

/*******************************************************************************
Función:     Reportar_Error
Descripcion: La funcion se llama si la variable 'OK' ha se modificó indicando
             una falla. Segun el valor de la variable se envia por el modulo
             UART la causa del fallo.
Resultado:   Se modifica la variable OK lo que hara que el PIC salga del loop
             principal.
*******************************************************************************/
void Reportar_Error(){
  if (OK==10){
   UART1_WRITE_TEXT("\n\rREVISAR MICROSWITCH DE ADELANTE");
  }
  if (OK==20){
   UART1_WRITE_TEXT("\n\rREVISAR MICROSWITCH DE ATRAS");
  }
  PORTE.RE0=1;
}

/*******************************************************************************
Función:     main
Descripcion: La funcion se llama si la variable 'OK' ha se modificó indicando
             una falla. Segun el valor de la variable se envia por el modulo
             UART la causa del fallo.
*******************************************************************************/
void main() {
  Inicializar();
  OK=1;
  Revisar_Microswitches();

  while (OK==1){

    if (procesar_comandos){Lobby();}
    if (automatico && sampled && encendido){Control_PID();}

  }

  PARAR()
  PWM(0)
  Reportar_Error();                   //Se envia un reporte del fallo

}

//************************Rutina de Servicio de Interrupción********************
void Interrupt() {

  if (PIR1.SSPIF == 1){

    PIR1.SSPIF = 0;
    if (SSPSTAT.D_A==1 && procesar_comandos==0){
      if (cnti2c==0){
        comando[0] = SSPBUF;
        //UART1_Write(comando[0]);
        cnti2c=1;
      }
      else {
        comando[1] = SSPBUF;
        //UART1_Write(comando[1]);
        procesar_comandos=1;
        cnti2c=0;
      }
    }
    tempi2c = SSPBUF;
  }

  if (PIR1.RC1IF) {
    if (cntuart==0){
      comando[0]=UART1_READ();        //Leer dato recibido
      //UART1_Write(comando[0]);
      cntuart=1;
    }
    else{
      comando[1]=UART1_READ();
      //UART1_Write(comando[1]);
      procesar_comandos=1;
      PORTD.RD2=~PORTD.RD2;
      cntuart=0;
    }
  }

if (PIR3.RC2IF) {
    uart20=UART2_READ();
    if (next==2){
      P2=uart20;
      presion_actual=P1*256+P2;
      next=0;
      sampled=1;
      PORTA.RA1=~PORTA.RA1;
    }
    else if (next==1){
      P1=uart20;
      next=2;
    }
    else if (uart22==6 && uart21==3 && uart20==2){
       next=1;
    }
    else {
       uart22=uart21;
       uart21=uart20;
    }
  }

  if ((INTCON.RBIF==1) && (INTCON.RBIE==1)){
      portbtemp=PORTB;
    if ((portbtemp.b4==0) || (portbtemp.b5==0)){
      //Si el cambio fue de bajada en RB1 o RB5
      PARAR()                         //Se para el motor
      PWM(0);                         //Ciclo de trabajo del PWM se hace cero.
      CCP1CON.DC1B1=0;
      CCP1CON.DC1B0=0;
      }
    //uart1_write('!');

    if (portbtemp.b4==0){cuartos=0;}

    TMR0H=0xC2;                       //Se inicia el TMR0 para que no afecte                                      el rebote
    TMR0L=0x0B;
    INTCON.TMR0IF=0;                  //Limpiar bandera de interrupcion del TMR0
    INTCON.TMR0IE=1;                  //Habilitar interrupción del TMR0
    INTCON.RBIF=0;                    //Limpiar bandera del puerto B
    INTCON.RBIE=0;                    //Habilitar interrupciones dle puerto B.
  }

  if (PIR2.CCP2IF) {
    TMR1H = 0;                        //Se reinicia el TMR1 a 0
    TMR1L = 0;
    PIR2.CCP2IF=0;                    //Limpar bandera de interrupcion
    PORTA.RA0=~PORTA.RA0;             //Cambiar estado del LED
    tempshort2=1;
    if(PORTA.RA7){cuartos--;}
    if(PORTA.RA6){cuartos++;}
  }

  if ((INTCON.TMR0IF==1) && (INTCON.TMR0IE==1)){
    INTCON.TMR0IF = 0;                //Limpiar bandera de interrupcion
    INTCON.TMR0IE = 0;                //Deshabilitar la interrupcion
    INTCON.RBIF=0;                    //Limpiar bandera de interrupcion
    INTCON.RBIE=1;                    //Habilitar interrupcion del puerto B
  }
}